#ifndef __ROBOT_PACOHEAD_CHEADCLIENT_H__
#define __ROBOT_PACOHEAD_CHEADCLIENT_H__

#include <robot/robot.h>
#include <robot/pacohead/enums.h>
#include <robot/pacohead/cheadpose.h>
#include <blackbox/bbox.h>
#include <blackbox/proxies.h>
#include <blackbox/cdatain.h>

namespace buola { namespace robot { namespace pacohead {

using namespace buola;

class CHeadClient : public blackbox::BBox
{
public:
    CHeadClient();
    ~CHeadClient();

    void Init(const blackbox::CBoxProxy &pBox=blackbox::CClientProxy("bbhead")["head"]);

    void LookAt(const mat::CVec3d &pPos,EMode pMode=pacohead::LOOKAT_DEFAULT);
    
    void GetPose(std::vector<double> &pValues);
    void GetPose(CHeadPose &pPose);

    void SetGoal(const mat::CVec3d &pPos);
    void SetGoal(const CHeadPose &pPos);
    void SetGoal(const std::vector<double> &pJoints);
    void SetGoal(int pJoint,double pValue);

    void Stop();
    void DoReset();

    bool IsMoving();
    void WaitForStop();

private:
    std::unique_ptr<blackbox::CBoxProxy> mHead;
    blackbox::CArrayIn<double> iJoints;
    blackbox::CDataIn<int> iStatus;
};

/*namespace pacohead*/ } /*namespace robot*/ } /*namespace buola*/ }

#endif
